#include "picture.h"

#include <algorithm>
#include <fstream>
#include <intrin.h>
#include <memory>
#include <omp.h>
#include <sstream>

#include "timer.h"

#include "exif_reader/exif_reader.h"
#include "glog/logging.h"

using std::endl;
using std::string;

// BRISK descriptor has 64 bytes
int Descriptor::desc_legth = 64;

//calculate the distance between the two sift descriptor
int CalculateSIFTDistanceSquared(const unsigned char* d1, const unsigned char* d2) {
  int dif, distsq = 0;
  for (int i = 0; i < 128; i++) {
    dif = d1[i] - d2[i];
    distsq += dif*dif;
  }
  return distsq;
}

uint64_t CalculateHammingDistance(const unsigned char* d1,
                                  const unsigned char* d2,
                                  int length = Descriptor::desc_legth) {
  uint64_t d1_, d2_;
  uint64_t ret = 0;
  for (; length > 0; length -= 8) {
    std::memcpy(&d1_, d1, 8);
    std::memcpy(&d2_, d2, 8);
    ret += __popcnt64(d1_^d2_);
    d1 += 8;
    d2 += 8;
  }
  return ret;
}

// for class Descriptor
Descriptor::Descriptor(const Descriptor& d) {
  if (d.ptrDesc != 0) {
    ptrDesc = nullptr;
    while (!ptrDesc) {
      ptrDesc = new unsigned char[desc_legth];
    }
    std::memcpy(ptrDesc, d.ptrDesc, desc_legth);
  }
}

Descriptor& Descriptor::operator=(const Descriptor& d) {
  if (this != &d) {
    ClearDesc();
    if (d.ptrDesc != 0) {
      ptrDesc = new unsigned char[desc_legth];
      std::memcpy(ptrDesc, d.ptrDesc, desc_legth);
    }
  }
  return (*this);
}

//move assignment 
Descriptor& Descriptor::operator=(Descriptor&& d) {
  if (this != &d) {
    ClearDesc();
    ptrDesc = d.ptrDesc;
    d.ptrDesc = nullptr;
  }
  return (*this);
}

// copy descriptor
unsigned char* Descriptor::CopyDescData(unsigned char* dst) const {
  if (ptrDesc) {
    std::memcpy(dst, ptrDesc, desc_legth);
    std::advance(dst, desc_legth);
  } else {
    LOG(ERROR) << "descriptor copy error, nullptr";
  }
  return dst;
}


/****** for class PICTURE ****/
//default constructor
Picture::Picture() : mImage_height(0), mImage_width(0), mIndex(-1), mDdImageFlag(true) {
	
}

Picture::~Picture() {

};

//set and get image size
void Picture::SetImageSize(const int width, const int height)
{
  mImage_width = width;
  mImage_height = height;
}

//get size
void Picture::GetImageSize(int& width, int& height) const
{
  height = mImage_height;
  width = mImage_width;
}

//clear all the data, delete the pointer content
void Picture::ClearData()
{
	mDescriptors.clear();
	mKeypoints.clear();
}

unsigned char* Picture::CopyDescData(unsigned char* dst) const {
  for (const auto & desc : mDescriptors) {
    dst = desc.CopyDescData(dst);
  }
  return dst;
}

//load  key points and descriptors  
bool Picture::LoadFeat( const std::string& key_file )
{
  mKeypoints.clear();
  mKeypoints.reserve( 10000 );
  std::ifstream infile( key_file, std::ios::in | std::ios::_Nocreate );
  if ( !infile.is_open() ) {
    LOG( ERROR ) << " key file open fail: " << key_file;
    return false;
  }
  float x, y, s, o;
  while ( infile >> x ) {
    infile >> y >> s >> o;
    mKeypoints.emplace_back( x, y, s, o );
  }
  VLOG( 2 ) << key_file << " load key points number: " << mKeypoints.size();
  infile.close();
}

bool Picture::LoadDesc( const std::string& desc_file )
{
  mDescriptors.clear();
  mDescriptors.resize( 10000 );
  std::ifstream infile( desc_file, std::ios::binary | std::ios::_Nocreate );
  if ( !infile.is_open() ) {
    LOG( ERROR ) << " desc file open fail: " << desc_file;
    return false;
  }

  std::size_t cardDesc = 0;
#if _RUNNING_MODE_  
  infile.read( (char*)&cardDesc, sizeof( std::size_t ) );
  CHECK_EQ( cardDesc, mKeypoints.size() ) << "descriptors and keypoints num error";
#else
  cardDesc = mKeypoints.size();
#endif
  mDescriptors.resize( cardDesc );
  for ( int i = 0; i < cardDesc; ++i ) {
    mDescriptors[i].ptrDesc = new unsigned char[Descriptor::desc_legth];
    infile.read( (char*)mDescriptors[i].ptrDesc, Descriptor::desc_legth );  // read 64 bytes
  }
  if ( infile.bad() ) {
    LOG( ERROR ) << "read desc fail";
    return false;
  }

  VLOG( 2 ) << "load descriptors number: " << mDescriptors.size();
  return true;
}

bool Picture::LoadKeyPointAndDesc(const std::string& filename, bool bLoadDesc)
{
  std::string key_file { filename };
  key_file.replace(key_file.end() - 3, key_file.end(), "feat");

  std::string desc_file { filename };
  desc_file.replace(desc_file.end() - 3, desc_file.end(), "desc");

  if ( !LoadFeat( key_file ) ) {
    return false;
  }

  if (!bLoadDesc) return true;

  return LoadDesc( desc_file );
}



/****** for Class AllPictures ***/
//set all empty
AllPictures::AllPictures() :mKeyfile_path(""),
mImage_path(""), mPicture_list_file("") {
  ClearPicture();
}

//set separated path 
AllPictures::AllPictures(const std::string& key_path, const std::string& list_file, const std::string& image_path)
  : mKeyfile_path(key_path), mPicture_list_file(list_file), mImage_path(image_path) {

}

//destructor
AllPictures::~AllPictures(){

}

//if there already exists pictures, clear them then reload
bool AllPictures::LoadPictures(const bool load_only_query) {
	//first clear all pictures if already loaded
	ClearPicture();
  Timer timer;

  std::ifstream infile(mPicture_list_file, std::ios::binary | std::ios::_Nocreate);
	if (!infile.is_open()) {
    LOG(ERROR) << "Open list file fail: " + mPicture_list_file;
		return false;
	}

	std::vector<std::string> pic_filename;
  pic_filename.reserve(1000);
  std::vector<int> pic_sizes;
  pic_sizes.reserve(3000);
  mParameters.clear();
  mParameters.reserve(3000);

  std::string line_in_file;
	std::string	picture_filename;
  int db, w, h;
  int cnt_loaded = 0;
  float ppx, ppy, f;
	while ( getline(infile, line_in_file) ){
		std::istringstream iss(line_in_file);
		iss >> picture_filename;
    iss >> db >> w >> h;
    pic_filename.emplace_back(picture_filename);
    pic_sizes.emplace_back(db);
    pic_sizes.emplace_back(w);
    pic_sizes.emplace_back(h);
    if (!load_only_query || (load_only_query && !db)) {
      ++cnt_loaded;
    }
    iss >> ppx >> ppy >> f;
    mParameters.push_back(ppx);
    mParameters.push_back(ppy);
    mParameters.push_back(f);
	}
	infile.close();
  VLOG(1) << "load picture names number: " << pic_filename.size();
  VLOG(1) << "pictures load desc number: " << cnt_loaded;
  CHECK_EQ(pic_sizes.size(), mParameters.size()) << "size not same, error";

	//clear the pictures and then load pictures;
  mPictures.resize(pic_filename.size());
#pragma omp parallel for
  for (int i = 0; i < pic_filename.size(); ++i) {
		//load a picture
    mPictures[i].SetFlag(pic_sizes[3 * i]);
    mPictures[i].SetImageSize(pic_sizes[3 * i + 1], pic_sizes[3 * i + 2]);
    mPictures[i].SetIndex(i);
    mPictures[i].SetPictureName(pic_filename[i]);
    // always load desc
    if (!load_only_query || (load_only_query && !mPictures[i].Flag())) {
      mPictures[i].LoadKeyPointAndDesc(mKeyfile_path + "/" + pic_filename[i], true);
    }
	}

  LOG(INFO) << "load picture keys and desc time: " << timer;
	return true;
}

/*
// load the camera pose ground truth from bundler.out and feature 3d info
bool AllPictures::LoadCamerasFromBundler(const std::string& bundler_out, Feature3DInfo& feat_insfo) {
	Timer timer;
  std::ifstream ifs(bundler_out, std::ios::binary);
	if (!ifs.is_open()) {
		LOG(ERROR) << "open bundler fail: " + bundler_out;
    return false;
	}
	std::string line;
	std::getline(ifs, line);  // header

	int num_cam = 0, num_points = 0;
	ifs >> num_cam >> num_points;
	mCameras.resize(num_cam);

  CHECK_EQ(mCameras.size(), mPictures.size()) << " cameras and pictures number error";

  cv::Matx33d D { 1., 0., 0., 0., 1., 0., 0., 0., -1. };
	for (size_t i = 0; i < num_cam; i++){
		ifs >> mCameras[i].f >> mCameras[i].k[0] >> mCameras[i].k[1];
    auto& r = mCameras[i].r;
    ifs >> r(0, 0) >> r(0, 1) >> r(0, 2)
      >> r(1, 0) >> r(1, 1) >> r(1, 2)
      >> r(2, 0) >> r(2, 1) >> r(2, 2);
    auto& t = mCameras[i].t;
    ifs >> t[0] >> t[1] >> t[1];
    // z axis of bundler is opposite to the usual z axis 
    r = D*r;
    t = D*t;
	}
	ifs.close();

  LOG(INFO) << "load cameras time: " << timer;
	return true;
}
*/

